#include "../src/Uart5.h"
#include "../src/PanelComm.h"
#include "../src/Common.h"

bool IsCommValid;
bool IsCommValid_CK;
PanelCommRecPack PanelCommRecPacket;
PanelCommRecPack PanelCommRecPacket_CK;
PanelCommRplyPack PanelCommRplyPacket;

static uint16_t IdxModLen(uint16_t idx);

void PanelComm_Init(void)
{
    PanelRxHead = 0;
    PanelRxTail = 0;
    PanelTxHead = 0;
    PanelTxTail = 0;

    PanelRxHead_CK = 0;
    PanelRxTail_CK = 0;
    PanelTxHead_CK = 0;
    PanelTxTail_CK = 0;
    
    IsCommValid = false;
    IsCommValid_CK = false;
}

bool PanelComm_ChkAndClearPackValidFlag(void)
{
    if (IsCommValid) 
    {
        IsCommValid = false;
        return true;
    } 
    else 
    {
        return false;
    }
}

void PanelComm_BuildRecPack(void)
{
    while (true) 
    {
        uint8_t rxLen;
        if (PanelRxTail >= PanelRxHead) 
        {
            rxLen = PanelRxTail - PanelRxHead;
        } 
        else
        {
            rxLen = (PANEL_BUF_LEN - PanelRxHead) + PanelRxTail;
        }

        if (rxLen < 9)
        {
            return;
        }

        PanelCommRecPacket.PackHead = PanelRxData[PanelRxHead];
        if (PanelCommRecPacket.PackHead != 0xAA) 
        {
            PanelRxHead = IdxModLen(PanelRxHead + 1);
            continue;
        }

        PanelCommRecPacket.PackLength = (PanelRxData[IdxModLen(PanelRxHead + 4)] << 8) | PanelRxData[IdxModLen(PanelRxHead + 3)];
        if (PanelCommRecPacket.PackLength > 73) 
        {
            PanelRxHead = IdxModLen(PanelRxHead + 1);
            continue;
        }

        if (rxLen < PanelCommRecPacket.PackLength + 2)
        {
            return;
        }

        PanelCommRecPacket.PackTail = PanelRxData[IdxModLen(PanelRxHead + PanelCommRecPacket.PackLength + 1)];
        if (PanelCommRecPacket.PackTail != 0xBB)
        {
            PanelRxHead = IdxModLen(PanelRxHead + 1);
            continue;
        }

        PanelCommRecPacket.PackIndex = PanelRxData[IdxModLen(PanelRxHead + 1)];
        PanelCommRecPacket.StationNum = PanelRxData[IdxModLen(PanelRxHead + 2)];
        PanelCommRecPacket.Crc = (PanelRxData[IdxModLen(PanelRxHead + PanelCommRecPacket.PackLength)] << 8) | PanelRxData[IdxModLen(PanelRxHead + PanelCommRecPacket.PackLength - 1)];

        uint16_t calCrc = CRC16_Chk_Rx((IdxModLen(PanelRxHead + 1)), PanelCommRecPacket.PackLength - 2);
        if (calCrc != PanelCommRecPacket.Crc) 
        {
            PanelRxHead = IdxModLen(PanelRxHead + 1);
            continue;
        }

        PanelCommRecPacket.Cmd = PanelRxData[IdxModLen(PanelRxHead + 5)];
        PanelCommRecPacket.DataByteLength = PanelCommRecPacket.PackLength - 7;
        
        int i;
        for (i = 0; i < PanelCommRecPacket.DataByteLength; i++) 
        {
            PanelCommRecPacket.Data[i] = PanelRxData[IdxModLen(PanelRxHead + 6 + i)];
        }

        IsCommValid = true;

        PanelRxHead = IdxModLen(PanelRxHead + PanelCommRecPacket.PackLength + 2);
        break;
    }
}
void PanelComm_BuildRecPack_ck(void)
{
    while (true) 
    {
        uint8_t rxLen_CK;
        if (PanelRxTail_CK >= PanelRxHead_CK) 
        {
            rxLen_CK = PanelRxTail_CK - PanelRxHead_CK;
        } 
        else
        {
            rxLen_CK = (PANEL_BUF_LEN - PanelRxHead_CK) + PanelRxTail_CK;
        }

        if (rxLen_CK < 9)
        {
            return;
        }

        PanelCommRecPacket_CK.PackHead = PanelRxData_CK[PanelRxHead_CK];
        if (PanelCommRecPacket_CK.PackHead != 0xAA) 
        {
            PanelRxHead_CK = IdxModLen(PanelRxHead_CK + 1);
            continue;
        }

        PanelCommRecPacket_CK.PackLength = (PanelRxData_CK[IdxModLen(PanelRxHead_CK + 4)] << 8) | PanelRxData_CK[IdxModLen(PanelRxHead_CK + 3)];
        if (PanelCommRecPacket_CK.PackLength > 73) 
        {
            PanelRxHead_CK = IdxModLen(PanelRxHead_CK + 1);
            continue;
        }

        if (rxLen_CK < PanelCommRecPacket_CK.PackLength + 2)
        {
            return;
        }

        PanelCommRecPacket_CK.PackTail = PanelRxData_CK[IdxModLen(PanelRxHead_CK + PanelCommRecPacket_CK.PackLength + 1)];
        if (PanelCommRecPacket_CK.PackTail != 0xBB)
        {
            PanelRxHead_CK = IdxModLen(PanelRxHead_CK + 1);
            continue;
        }

        PanelCommRecPacket_CK.PackIndex = PanelRxData_CK[IdxModLen(PanelRxHead_CK + 1)];
        PanelCommRecPacket_CK.StationNum = PanelRxData_CK[IdxModLen(PanelRxHead_CK + 2)];
        PanelCommRecPacket_CK.Crc = (PanelRxData_CK[IdxModLen(PanelRxHead_CK + PanelCommRecPacket_CK.PackLength)] << 8) | PanelRxData_CK[IdxModLen(PanelRxHead_CK + PanelCommRecPacket_CK.PackLength - 1)];

        uint16_t calCrc_CK = CRC16_Chk_Rx_CK((IdxModLen(PanelRxHead_CK + 1)), PanelCommRecPacket_CK.PackLength - 2);
        if (calCrc_CK != PanelCommRecPacket_CK.Crc) 
        {
            PanelRxHead_CK = IdxModLen(PanelRxHead_CK + 1);
            continue;
        }

        PanelCommRecPacket_CK.Cmd = PanelRxData_CK[IdxModLen(PanelRxHead_CK + 5)];
        PanelCommRecPacket_CK.DataByteLength = PanelCommRecPacket_CK.PackLength - 7;
        
        int i;
        for (i = 0; i < PanelCommRecPacket_CK.DataByteLength; i++) 
        {
            PanelCommRecPacket_CK.Data[i] = PanelRxData_CK[IdxModLen(PanelRxHead_CK + 6 + i)];
        }

        IsCommValid_CK = true;

        PanelRxHead_CK = IdxModLen(PanelRxHead_CK + PanelCommRecPacket_CK.PackLength + 2);
        break;
    }
}
static uint16_t IdxModLen(uint16_t idx) 
{
    return idx & (PANEL_BUF_LEN - 1);
}

void PanelComm_FillTxBytesWithRplyPack(void)
{
    PutByteInTxBuf(PanelCommRplyPacket.PackHead, 0);
    PutByteInTxBuf(PanelCommRplyPacket.PackIndex, 1);
    PutByteInTxBuf(PanelCommRplyPacket.StationNum, 2);
    PutByteInTxBuf(PanelCommRplyPacket.PackLength, 3);
    PutByteInTxBuf(PanelCommRplyPacket.PackLength >> 8, 4);
    PutByteInTxBuf(PanelCommRplyPacket.Cmd, 5);

    uint8_t i;
    for (i = 0; i < PanelCommRplyPacket.DataByteLength; i++)
    {
        PutByteInTxBuf(PanelCommRplyPacket.Data[i], 6 + i);
    }

    uint16_t crc = CRC16_Chk_Tx(IdxModLen(PanelTxTail + 1), PanelCommRplyPacket.PackLength - 2);
    PutByteInTxBuf(crc, 6 + PanelCommRplyPacket.DataByteLength);
    PutByteInTxBuf(crc >> 8, 6 + PanelCommRplyPacket.DataByteLength + 1);
    PutByteInTxBuf(PanelCommRplyPacket.PackTail, 6 + PanelCommRplyPacket.DataByteLength + 2);

    PanelTxTail = IdxModLen(PanelTxTail + 7 + PanelCommRplyPacket.DataByteLength + 2);
}

void RotateAxis_FillTxBytesWithRplyPack(void)
{
    PanelTxData_CK[0] = PanelCommRecPacket.PackHead;
    PanelTxData_CK[1] = PanelCommRecPacket.PackIndex;
    PanelTxData_CK[2] = PanelCommRecPacket.StationNum;
    PanelTxData_CK[3] = PanelCommRecPacket.PackLength;
    PanelTxData_CK[4] = PanelCommRecPacket.PackLength >> 8;
    PanelTxData_CK[5] = PanelCommRecPacket.Cmd;
    uint8_t i;
    for (i = 0; i < PanelCommRecPacket.DataByteLength; i++)
    {
        PanelTxData_CK[6 + i] = PanelCommRecPacket.Data[i];
    }
    PanelTxData_CK[6 + PanelCommRecPacket.DataByteLength] = PanelCommRecPacket.Crc;
    PanelTxData_CK[6 + PanelCommRecPacket.DataByteLength + 1] = PanelCommRecPacket.Crc >> 8;
    PanelTxData_CK[6 + PanelCommRecPacket.DataByteLength + 2] = PanelCommRecPacket.PackTail;

}
void PutByteInTxBuf(uint8_t byte, uint16_t offset) {
    PanelTxData[IdxModLen(PanelTxTail + offset)] = byte;
}
void PutByteInTxBuf_CK(uint8_t byte, uint16_t offset) {
    PanelTxData_CK[IdxModLen(offset)] = byte;
}